

from ArmMove import ArmMover
import rospy, sys
from geometry_msgs.msg import PoseStamped, Pose
import moveit_commander
from moveit_commander import MoveGroupCommander
from copy import deepcopy
import socket
import numpy as np
from math import pi
from time import sleep
import GripperControl
from RPYPose import RPYPoser,SIYUANPoser

# grap为设置正向 loose为设置反向 每发送一次 均为单次执行 
# 若夹红枣 只需单次 杯子需要双次 
gripper_grap = b'\x01\x06\x00\x0b\x00\x01\x39\xc8'
gripper_loose = b'\x01\x06\x00\x0b\x00\x00\xf8\x08'

# 弧度转角度
RA2DE = 180.0 / pi
# 角度转弧度
DE2RA = pi / 180.0

# 夹爪字典
name = '' # 'chenpi' 'hongzao' 'jiangtang' 'puer' 'qingju' 'meigui' 

'''
先Init_State() 识别 
识别得到msg name后 
进行杯子抓取Beizi_Grap()

根据配茶秘方for 循环：
    进行 Openbox() 然后识别物体
    识别得到msg name后 
    Cailiao_Grap() 抓取物体
    进行CloseBox()

把水杯移动到饮水机 Beizi_To_Drink()
打开饮水机 Push_Drink()
移动茶品到顾客处 Get_Drink()

'''

def Init_State():
    '''
    初始化
    固定位置进行识别杯子 爪子张开
    '''
    Arm = ArmMover()
    joints_look = [-60.67638917551703, 30.958930588437717, 66.30949807520462, -24.253970457265563, 88.6308377313422, 143.5199429315394]
    Arm.MovebyJoint(joints_look)
    GripperControl.newcontrol(0)

def Control(msg,name):
    '''
    控制抓取过程
    :param msg: 识别到的坐标(x,y,z,r,p,y) 待更改
    :param name: 识别到的标签
    '''
    # 初始化抓取时对应姿态 包含夹爪角度
    joints = [0, 0, 0, 0, 0, 0]

    # 全局化记忆
    global joints_beizi_end
    global joints_beizi_start

    # 处理msg与name 
    if name == "beizi":
        ## 抓取杯子
        # 初始化掉落点前的对应角度
        joints_down = [-4.046883011209535, -24.360175419897672, 51.41464726565747, -14.200682276065276, 94.50595899515324, 74.57322971675787]
        # 根据位置信息获取关节姿态实例
        # 在test中 直接调joint进行设置
        joints_pose = SIYUANPoser(msg).SIYUANtoPose()
        # 进行杯子抓取
        Beizi_Grap(joints_pose, joints_down)

    else:
        ## 抓取物品
        # 初始化掉落点前的对应姿态
        joints_down = [-1.6187671977891651, -38.08107011574237, 3.0086032451056637, -43.43213859732666, 95.63002275877926, 77.46779396184895]
        # 根据位置信息获取关节姿态实例
        joints_pose = SIYUANPoser(msg).SIYUANtoPose()
        # 进行材料抓取
        Cailiao_Grap(joints_pose, joints_down, name)

def Beizi_Grap(joints_pose, joints_down):
    '''
    移动过程
    夹爪初始 2松 结束 2松
    :param joints: 杯子起点
    :param joints_down: 杯子终点 固定即可
    '''
    print("now: beizi graping !")

    # 初始化机械臂
    Arm = ArmMover()
    # 初始化夹爪
    GripperControl.newcontrol(0)

    # 初始位置
    joints_look =  [-60.67638917551703, 30.958930588437717, 66.30949807520462, -24.253970457265563, 88.6308377313422, 143.5199429315394]
    # 抬起
    joints_beizi_0 = [-1.6187671977891651, -38.08107011574237, 3.0086032451056637, -43.43213859732666, 95.63002275877926, 77.46779396184895]
    joints_up = [20.37368914274531, -21.55171909497705, 32.87061483033298, -35.563209627034915, 89.92346654814281, 49.20774757198476]
    
    # 移动至物体位置上方
    Arm.MovebyJoint(joints_look,0.4)
    # 移动至物体位置
    Arm.MovebyPose(joints_pose,0.2)
    # 进行抓取,夹紧夹爪
    GripperControl.newcontrol(1)
    sleep(0.5)

    # 抬起至对应位置上方
    j =  [joints_down[0],-32, 16, -42, 90, 23]
    Arm.MovebyJoint(j,0.5)

    # 抬起至对应位置
    Arm.MovebyJoint(joints_down)

    # # 释放物体,松开夹爪
    GripperControl.newcontrol(0)
    sleep(0.5)

    # 抬起
    Arm.MovebyJoint(joints_beizi_0)

    #归中 接开箱操作
    Arm.MovebyJoint(joints_up,0.5)

def Cailiao_Grap(joints_pose, joints_down, name):
    '''
    夹爪 初始1松 结束1松
    :param joints_pose: 材料起点
    :param joints_down: 材料终点 固定即可
    :param name: 材料名称 根据name的不同选择jointslook joints_cailiao_0

    '''
    print("cailiao graping !")

    Arm = ArmMover()
    GripperControl.newcontrol(2)

    # 总览
    joints_look_all = [20.37368914274531, -21.55171909497705, 32.87061483033298, -35.563209627034915, 89.92346654814281, 49.20774757198476]
    # 查找对应的材料的识别位置
    if name == 'chenpi':
        joints_look = [76.94363208733526, 16.29648428658548, 99.91687328854839, 12.224516750904282, 84.27566382307032, 86.06979785994304]
        joints_cailiao_0 =  [75.91638785227991, -24.9242251637064, 53.52096189408959, 0.17085086321800402, 91.74305014857177, 104.45152658124331]
    elif name == 'hongzao':
        joints_look = [39.05303894789994, -14.983076142880083, 32.36152486437031, -25.426913268924714, 96.29906817407995, 105.96071743524513]
        joints_cailiao_0 =  [42.60462696530226, -53.35797446027863, 6.194743677481124, -28.611389566284792, 88.72114100152652, 57.83363881360132]
    elif name == 'jiangtang':
        joints_look = [36.26718797748869, -31.787686072887187, 5.32424205197828, -28.732074531265887, 91.19551567882215, 104.10368125339096]
        joints_cailiao_0 =  [50.245741007315175, -40.3007617621459, 17.788318301566054, -13.608636038319545, 56.8533559317375, 77.11624232593552]
    elif name == 'puer':
        joints_look = [-70.55285454033132, -2.5391522153567463, -110.77921508948857, -48.18797398110102, -101.18574282329703, 77.18020855393655]
        joints_cailiao_0 =  [-84.3808746994274, 22.147274367191425, -59.85688592780998, 2.676960386122316, -86.89433250253882, 108.07222620465744]
    elif name == 'qingju':
        joints_look = [-42.57587155686836, 43.636082810308984, 31.21475449443847, 68.73572397043739, -90.71708906366561, 99.1708766628771]
        joints_cailiao_0 =  [-39.56409294194426, 53.14785239157151, -8.102547075794936, 22.696378763149596, -91.51796419513033, 128.22768559384028]
    elif name == 'meigui':
        joints_look = [-41.84353150979235, 35.19294870307727, 19.52862003461151, 50.14991325089466, -93.44378135070984, 99.31342439621388]
        joints_cailiao_0 =  [-45.01588466573446, 41.94566518487515, -10.451904438335118, 19.948970091501224, -73.66788967300666, 124.35522967570756]


    # 移动至物体位置7
    Arm.MovebyPose(joints_pose,0.1)

    # # 进行抓取,夹紧夹爪
    GripperControl.newcontrol(1)
    sleep(0.5)

    # 抬起至对应位置上方
    Arm.MovebyJoint(joints_cailiao_0,0.3)

    # 抬起至对应位置
    Arm.MovebyJoint(joints_down,0.3)

    # # 释放物体,松开夹爪
    GripperControl.newcontrol(0)

    # 抬起
    Arm.MovebyJoint(joints_look_all,0.3)
    sleep(0.5)

<<<<<<< HEAD
    # 到达下一识别位置
    Arm.MovebyJoint(joints_look,0.3)


=======
>>>>>>> 7ab8b32aacdca90fbd07e836fad97c25c5aa0f47
def Open_Box(name,list_name):
    '''
    固定位置开箱 
    初始 夹爪1松 结束 1松
    :param name:识别材料名称 根据name的不同选择joints0 joints1 jointsbox0 jointsbox1
    :param list_name:按位置摆放的材料
    '''
    print("now : opening box !")

    # 机械臂初始化
    Con = ArmMover()
    # 夹爪初始化
    # GripperControl.send(gripper_grap)
    # sleep(0.5)
<<<<<<< HEAD
    end_flag = 0
    if name == list_name[0]:
        joints_look = [76.94363208733526, 16.29648428658548, 99.91687328854839, 12.224516750904282, 84.27566382307032, 86.06979785994304]
=======

    if name == list_name[0]:
        joints_look = [49.31785164387933, 4.494188325397365, 57.262446947811206, -19.21657554034187, 93.188317371393, 115.76970575904467]
>>>>>>> 7ab8b32aacdca90fbd07e836fad97c25c5aa0f47
        # 根据name可调
        joints_box_0 = [28.132997977398265, -13.243436618777928, 75.25728484574512, -99.27203664242067, -154.29570231352997, 86.85884196326462]
        joints_box_1 = [28.132997977398265, -13.243436618777928, 75.25728484574512, -99.27203664242067, -154.29570231352997, 86.85884196326462]
        # 拉取扶手预备位置
        joints_0 = [56.590881674470204, -44.92881246750753, 82.90822069946535, -52.099563246724806, -125.71152727868652, 94.14940920861707]
        joints_1 = [32.6993417015198, -31.80866559443761, 97.03271057933273, -59.13049797907581, -145.57689910388908, 91.85197217540689]
        # 拉取起点终点
        joints_start = [61.563418230415756, -53.247733893805574, 61.513639467424944, -67.83100058748904, -116.6186983955697, 97.26690554736712]
        joints_end = [46.11548662991096, -35.94496838697015, 100.4647431328895, -46.74830970525101, -132.04700747826058, 96.40551940830395]
        end_flag = 1

    elif name == list_name[1]:
        joints_look = [39.05303894789994, -14.983076142880083, 32.36152486437031, -25.426913268924714, 96.29906817407995, 105.96071743524513]
        # 根据name可调
        joints_box_0 = [90.96976184635326, 1.4461970826575847, 98.99639997539316, 85.80525190352866, 4.0302822386527, 62.53982135113543]
        joints_box_1 = [90.96976184635326, 1.4461970826575847, 98.99639997539316, 85.80525190352866, 4.0302822386527, 62.53982135113543]
        # 拉取扶手预备位置
        joints_0 = [65.60503429436376, -40.89343686096325, 85.24905017589215, -61.299409393965014, -26.385626055509775, 111.60483811239148]
        joints_1 = [110.23560873365858, -28.74082501286636, 123.70857627525442, -17.2704643044231, 18.726867440812892, 95.03193135133657]
        # 拉取起点终点
        joints_start = [60.32465078155394, -53.46311351002266, 71.11588733499923, -61.702870187196154, -31.636153194068683, 110.29157048270565]
        joints_end = [87.1680434782245, -44.33987558972909, 99.4690836512773, -71.5504869670366, -5.689869046737791, 140.02781773411687]

    elif name == list_name[2]:
        joints_look = [36.26718797748869, -31.787686072887187, 5.32424205197828, -28.732074531265887, 91.19551567882215, 104.10368125339096]
        # 根据name可调
        joints_box_0 = [90.96976184635326, 1.4461970826575847, 98.99639997539316, 85.80525190352866, 4.0302822386527, 62.53982135113543]
        joints_box_1 = [90.96976184635326, 1.4461970826575847, 98.99639997539316, 85.80525190352866, 4.0302822386527, 62.53982135113543]
        # 拉取扶手预备位置
        joints_0 = [57.502406947854816, -43.69760870852455, 57.55575834314929, -77.110554445484, -29.138383301257914, 124.63921136276045]
        joints_1 = [102.0676946132626, -17.717198032046333, 112.00082070269704, -36.35335252349134, 14.16517898991094, 97.95021174997856]
        # 拉取起点终点
        joints_start = [55.23161185043341, -58.082498760842306, 40.164545583462, -80.29589581317823, -31.46440591291913, 110.26606324619873]
        joints_end = [79.70375290731371, -35.472140625406155, 88.07529469342802, -78.0437348636222, -9.24472408134642, 132.84738089296934]
    
    elif name == list_name[3]:
<<<<<<< HEAD
        joints_look = [-70.55285454033132, -2.5391522153567463, -110.77921508948857, -48.18797398110102, -101.18574282329703, 77.18020855393655]
=======
        joints_look = [-40.753023187419494, 36.68229208790225, 15.627365528872685, 64.38533129786627, -90.80828519378984, 108.18972582572516]
>>>>>>> 7ab8b32aacdca90fbd07e836fad97c25c5aa0f47
        # 根据name可调
        joints_box_0 = [-41.6021998012661, -5.501912950327209, -113.73862596038822, 70.72861725271528, 140.3482737292506, 108.71981884333651]
        joints_box_1 = [-41.6021998012661, -5.501912950327209, -113.73862596038822, 70.72861725271528, 140.3482737292506, 108.71981884333651]
        # 拉取扶手预备位置
        joints_0 = [-62.90600707702462, 47.31760763394238, -87.53396133602335, 44.412134792058545, 119.01443985593963, 109.09697398770953]
        joints_1 = [-38.270805530005966, 40.16069835543829, -112.96788458925259, 25.721944465427725, 143.6495378910664, 108.57134966760852]
        # 拉取起点终点
        joints_start = [-67.8346215551143, 56.29933858322614, -67.53900197048821, 55.46338592896067, 114.08562715018326, 109.17533285268979]
        joints_end = [-48.14177897793145, 41.90778622280469, -108.02132068976863, 29.13281504073732, 133.77832950868083, 108.83679680504076]

    elif name == list_name[4]:
        joints_look = [-42.57587155686836, 43.636082810308984, 31.21475449443847, 68.73572397043739, -90.71708906366561, 99.1708766628771]
        # 根据name可调
        joints_box_0 = [-91.78812753218035, -21.005125757969072, -84.63222165000006, 4.033883594788217, 5.8146355303261235, 68.31719458715988]
        joints_box_1 = [-91.78812753218035, -21.005125757969072, -84.63222165000006, 4.033883594788217, 5.8146355303261235, 68.31719458715988]
        # 拉取扶手预备位置
        joints_0 = [-65.01962202592907, 46.01214881768056, -92.06981961063104, 36.77555309860527, 25.503722197863617, 91.34759051154472]
        joints_1 = [-104.5689706772471, 43.21892419848777, -123.121895884477, 22.430115450100843, -14.311863396685643, 91.6040618004917]
        # 拉取起点终点
        joints_start = [-57.693525303904856, 58.00858852761728, -76.30515499931741, 41.59696292784344, 32.79744782948376, 103.45996160747508]
        joints_end = [-87.66880580643188, 51.00769919373861, -133.0750793029898, -42.638140131345935, 3.483046884614006, 138.58190447418568]

    elif name == list_name[5]:
        joints_look = [-41.84353150979235, 35.19294870307727, 19.52862003461151, 50.14991325089466, -93.44378135070984, 99.31342439621388]
        # 根据name可调
        joints_box_0 = [-91.78812753218035, -21.005125757969072, -84.63222165000006, 4.033883594788217, 5.8146355303261235, 68.31719458715988]
        joints_box_1 = [-91.78812753218035, -21.005125757969072, -84.63222165000006, 4.033883594788217, 5.8146355303261235, 68.31719458715988]
        # 拉取扶手预备位置
        joints_0 = [-56.91458906636432, 43.269322531743754, -65.99910954919467, 66.7626140533892, 33.607159574871375, 103.34614120558263]
        joints_1 = [-106.18046392844973, 23.597296328699663, -124.52960013641807, 39.62823938408572, -15.8613837633491, 92.63958357452978]
        # 拉取起点终点
        joints_start = [-50.49179502429021, 57.01038810851468, -47.65471464567712, 71.90255750430973, 40.054139520512265, 102.61840554823924]
        joints_end =  [-77.34095029193261, 26.987877898310185, -112.07590398663903, 31.587163497168913, 13.313769904957146, 109.08779343393525]
        end_flag = 1
        # [-81.63167769591732, 25.558777689896466, -110.06989137905025, 45.03472421933769, 8.444006976340106, 99.43297754924053]  # 0.1可

    # 拉取初始
    Con.MovebyJoint(joints_box_0,0.3)
    # 抓取预备起点
    Con.MovebyJoint(joints_0,0.3)
    # 起点
    Con.MovebyJoint(joints_start,0.3)
    # 夹爪加紧
    GripperControl.newcontrol(1)
    sleep(0.5)
    # 终点
    Con.MovebyJoint(joints_end,0.3 - end_flag*0.2)
    # 夹爪松
    GripperControl.newcontrol(0)
    # 后拉至关节预备终点点
    Con.MovebyJoint(joints_1,0.3)
    # 退回初始位置
    Con.MovebyJoint(joints_box_1,0.3)
    # 到达下一识别位置
    Con.MovebyJoint(joints_look,0.3)
 
def Close_Box(name,list_name):
    '''
    固定位置关箱 
    初始 夹爪0松 结束 1松
    :param name:材料名称 根据name的不同选择joints0 joints1 jointsbox0 jointsbox1
    :param list_name:按位置摆放的材料
    内值均与openbox反向 完全对照
    '''
    print("now: closing box !")

    # 机械臂初始化
    Con = ArmMover()

    if name == list_name[0]:
<<<<<<< HEAD
        joints_box_0 = [28.132997977398265, -13.243436618777928, 75.25728484574512, -99.27203664242067, -154.29570231352997, 86.85884196326462]
        joints_box_1 = [28.132997977398265, -13.243436618777928, 75.25728484574512, -99.27203664242067, -154.29570231352997, 86.85884196326462]
=======
        # 根据name可调
        joints_box_0 = [90.96976184635326, 1.4461970826575847, 98.99639997539316, 85.80525190352866, 4.0302822386527, 62.53982135113543]
        joints_box_1 = [90.96976184635326, 1.4461970826575847, 98.99639997539316, 85.80525190352866, 4.0302822386527, 62.53982135113543]
>>>>>>> 7ab8b32aacdca90fbd07e836fad97c25c5aa0f47
        # 拉取扶手预备位置
        joints_0 = [56.590881674470204, -44.92881246750753, 82.90822069946535, -52.099563246724806, -125.71152727868652, 94.14940920861707]
        joints_1 = [32.6993417015198, -31.80866559443761, 97.03271057933273, -59.13049797907581, -145.57689910388908, 91.85197217540689]
        # 拉取起点终点
        joints_start = [61.563418230415756, -53.247733893805574, 61.513639467424944, -67.83100058748904, -116.6186983955697, 97.26690554736712]
        joints_end = [42.68577722457199, -33.19330083276743, 99.25756889752329, -54.011923368230555, -135.65559061259034, 93.82608241452083]

    elif name == list_name[1]:
        joints_box_0 = [90.96976184635326, 1.4461970826575847, 98.99639997539316, 85.80525190352866, 4.0302822386527, 62.53982135113543]
        joints_box_1 = [90.96976184635326, 1.4461970826575847, 98.99639997539316, 85.80525190352866, 4.0302822386527, 62.53982135113543]
        # 拉取扶手预备位置
        joints_0 = [65.60503429436376, -40.89343686096325, 85.24905017589215, -61.299409393965014, -26.385626055509775, 111.60483811239148]
        joints_1 = [110.23560873365858, -28.74082501286636, 123.70857627525442, -17.2704643044231, 18.726867440812892, 95.03193135133657]
        # 拉取起点终点
        joints_start = [56.62008650943562, -58.85807931190627, 59.98336372232351, -66.83899505936205, -35.25078594696216, 109.62641860163437]
        joints_end = [87.1680434782245, -44.33987558972909, 99.4690836512773, -71.5504869670366, -5.689869046737791, 140.02781773411687]

    elif name == list_name[2]:
        # 根据name可调
        joints_box_0 = [90.96976184635326, 1.4461970826575847, 98.99639997539316, 85.80525190352866, 4.0302822386527, 62.53982135113543]
        joints_box_1 = [90.96976184635326, 1.4461970826575847, 98.99639997539316, 85.80525190352866, 4.0302822386527, 62.53982135113543]
        # 拉取扶手预备位置
        joints_0 = [57.502406947854816, -43.69760870852455, 57.55575834314929, -77.110554445484, -29.138383301257914, 124.63921136276045]
        joints_1 = [102.0676946132626, -17.717198032046333, 112.00082070269704, -36.35335252349134, 14.16517898991094, 97.95021174997856]
        # 拉取起点终点
        joints_start = [51.64692142796036, -72.91810038872438, 9.217116806523073, -96.46602758927037, -34.99270235519795, 110.40568848086932]
        joints_end = [79.70375290731371, -35.472140625406155, 88.07529469342802, -78.0437348636222, -9.24472408134642, 132.84738089296934]

    elif name == list_name[3]:
        # 根据name可调
        joints_box_0 = [-41.6021998012661, -5.501912950327209, -113.73862596038822, 70.72861725271528, 140.3482737292506, 108.71981884333651]
        joints_box_1 = [-41.6021998012661, -5.501912950327209, -113.73862596038822, 70.72861725271528, 140.3482737292506, 108.71981884333651]
        # 拉取扶手预备位置
        joints_0 = [-62.90600707702462, 47.31760763394238, -87.53396133602335, 44.412134792058545, 119.01443985593963, 109.09697398770953]
        joints_1 = [-38.270805530005966, 40.16069835543829, -112.96788458925259, 25.721944465427725, 143.6495378910664, 108.57134966760852]
        # 拉取起点终点
        joints_start = [-67.8346215551143, 56.29933858322614, -67.53900197048821, 55.46338592896067, 114.08562715018326, 109.17533285268979]
        joints_end = [-48.14177897793145, 41.90778622280469, -108.02132068976863, 29.13281504073732, 133.77832950868083, 108.83679680504076]

    elif name == list_name[4]:
        # 根据name可调
        joints_box_0 = [-91.78812753218035, -21.005125757969072, -84.63222165000006, 4.033883594788217, 5.8146355303261235, 68.31719458715988]
        joints_box_1 = [-91.78812753218035, -21.005125757969072, -84.63222165000006, 4.033883594788217, 5.8146355303261235, 68.31719458715988]
        # 拉取扶手预备位置
        joints_0 = [-65.01962202592907, 46.01214881768056, -92.06981961063104, 36.77555309860527, 25.503722197863617, 91.34759051154472]
        joints_1 = [-104.5689706772471, 43.21892419848777, -123.121895884477, 22.430115450100843, -14.311863396685643, 91.6040618004917]
        # 拉取起点终点
        joints_start = [-57.693525303904856, 58.00858852761728, -76.30515499931741, 41.59696292784344, 32.79744782948376, 103.45996160747508]
        joints_end = [-87.66880580643188, 51.00769919373861, -133.0750793029898, -42.638140131345935, 3.483046884614006, 138.58190447418568]
    
    elif name == list_name[5]:
        # 根据name可调
        joints_box_0 = [-91.78812753218035, -21.005125757969072, -84.63222165000006, 4.033883594788217, 5.8146355303261235, 68.31719458715988]
        joints_box_1 = [-91.78812753218035, -21.005125757969072, -84.63222165000006, 4.033883594788217, 5.8146355303261235, 68.31719458715988]
        # 拉取扶手预备位置
        joints_0 = [-56.91458906636432, 43.269322531743754, -65.99910954919467, 66.7626140533892, 33.607159574871375, 103.34614120558263]
        joints_1 = [-106.18046392844973, 23.597296328699663, -124.52960013641807, 39.62823938408572, -15.8613837633491, 92.63958357452978]
        # 拉取起点终点
        joints_start = [-50.49179502429021, 57.01038810851468, -47.65471464567712, 71.90255750430973, 40.054139520512265, 102.61840554823924]
        joints_end =  [-77.34095029193261, 26.987877898310185, -112.07590398663903, 31.587163497168913, 13.313769904957146, 109.08779343393525]
        
    # 拉取初始
    Con.MovebyJoint(joints_box_1,0.3)
    # 抓取预备起点
    Con.MovebyJoint(joints_1,0.3)
    # 起点
    Con.MovebyJoint(joints_end,0.3)
    # 夹爪加紧
    GripperControl.newcontrol(1)
    sleep(0.5)
    # 终点
    Con.MovebyJoint(joints_start,0.3)
    # 夹爪松
    GripperControl.newcontrol(0)
    sleep(0.5)
    # 后拉至关节预备终点点
    Con.MovebyJoint(joints_0,0.3)
    # 退回初始位置
    Con.MovebyJoint(joints_box_0,0.3)

def Beizi_To_Drink():
    
    print("now : moving beizi to drink  !")

    # 初始化机械臂
    Arm = ArmMover()
     # 初始化夹爪
    GripperControl.newcontrol(0)

    # 初始位置
    joints_look_0 = [45.24030863930541, -21.669151413962254, 10.56932921605124, -54.49550842369311, 94.62258883592305, 57.10411569307362]
    # 为下一动作【按压饮水机Push_Drink】的初始化动作
    joints_look_1 = [-18.35721598561729, 10.101406014558284, -25.763873223856027, 42.88951983194061, -89.14777385579383, 60.42179978284899]

    
    # 杯子预备位置
    joints_drink_0 = [ 42.85494107715227, -50.97346718235573, 50.196320602967646, -74.5048198757327, 42.793337369584286, 41.76684312382531]
    joints_drink_1 = [-79.24493798583275, -44.78447626588668, 73.85034967459715, -52.45126277096409, 46.236071543499854, 64.3282291367248]

    # 抓杯子过程位置
    joints_0 = [32.997183351479784, -43.416156667305685, 65.77677569149917, -65.37759470196129, 32.95781598689861, 40.381520479285065]
    # 略抬升位置
    joints_1 = [31.535846991877722, -52.32468393495886, 40.38652396837164, -81.49217376010192, 31.51293423592395, 46.06208915547212]
    # 旋转接近饮水机处
    joints_2 = [-87.3172889523259, -52.32142238152142, 40.3832476394609, -81.47707777231993, 39.855501570532404, 46.045216885501205]
    # 到达饮水机位置
    joints_3 = [-93.7617377557454, -31.82924466191599, 84.99398632981274, -56.37260399800603, 33.45098014696846, 44.82606361756877]

    
    # # 移动至物体位置上方
    Arm.MovebyJoint(joints_look_0,0.5)

    # 预备抓杯位置
    Arm.MovebyJoint(joints_drink_0,0.3)

    # 抓取位置
    Arm.MovebyJoint(joints_0,0.5)
    # 夹爪抓取
    GripperControl.newcontrol(1)
    sleep(0.5)
    # 略抬升位置
    Arm.MovebyJoint(joints_1,0.5)
    # 旋转接近饮水机处
    Arm.MovebyJoint(joints_2,0.2)  
    # 到达饮水机位置
    Arm.MovebyJoint(joints_3,0.5)
    # 夹爪松开
    GripperControl.newcontrol(0)
    sleep(0.5)
    # 后退位置
    Arm.MovebyJoint(joints_drink_1,0.5)

    # 抬起 为下一动作【按压饮水机Push_Drink】的初始化动作
    Arm.MovebyJoint(joints_look_1,0.2)

def Push_Drink():
    
    print("now : pushing drink !")

     # 初始化机械臂
    Arm = ArmMover()

    # 初始化夹爪
    GripperControl.newcontrol(0)

    # 初始位置
    joints_look = [-90,-32, 16, -42, 90, 23]
    # 识别杯子
    joints_drink_0 = [-18.35721598561729, 10.101406014558284, -25.763873223856027, 42.88951983194061, -89.14777385579383, 60.42179978284899]
    joints_drink_1 = [-18.35721598561729, 10.101406014558284, -25.763873223856027, 42.88951983194061, -89.14777385579383, 60.42179978284899]

    joints_0 = [7.157536648054807, 13.767147463301283, -44.37016393507476, 12.015213169062784, -92.36542603572808, 66.88363090726718]
    joints_1 = [5.563633072561074, 6.727235361772198, -62.63084012504152, 0.7412671800391712, -91.7935740910617, 65.32483780326977]
    joints_11= [7.157536648054807, 13.767147463301283, -44.37016393507476, 12.015213169062784, -92.36542603572808, 66.88363090726718]
    joints_2 = [7.497770858180807, 35.18360864906135, -5.62761775526084, 32.48033024637178, -95.94788706557503, 106.0901352166828]
    joints_3 = [9.23782476789352, 21.660501086496307, -37.178721556100726, 17.696449530959356, -95.47348073434101, 108.10082990233698]
    joints_4 = [8.895796794099375, 29.82939167216471, -12.369404252401708, 31.219684002078164, -96.35160938828012, 107.43711647936308]

   # 饮水机童锁上方
    Arm.MovebyJoint(joints_0,0.5)
    # 饮水机按钮按童锁
    Arm.MovebyJoint(joints_1,0.5)
    sleep(1)
    Arm.MovebyJoint(joints_11,0.5)
    sleep(1)
    # 饮水机开关上方
    Arm.MovebyJoint(joints_2,0.5)  
    # 饮水机按钮按250ml并开启
    Arm.MovebyJoint(joints_3,0.5)
    # 饮水机上方
    Arm.MovebyJoint(joints_4,0.5)
    # 抬起
    Arm.MovebyJoint(joints_drink_1,0.5)

def Get_Drink():
    # 初始化机械臂
    Arm = ArmMover()
    # 初始化夹爪
    GripperControl.newcontrol(0)
    sleep(0.5)

    # 初始位置
    joints_look = [-90,-32, 16, -42, 90, 23]
    # 杯子预备位置
<<<<<<< HEAD
    joints_drink_0 = [-80.41020607088991, -34.8470549319062, 74.18243584578649, -61.832637144726654, 45.022171536507656, 20.512724040675966]
    joints_drink_1 = [-90,-32, 16, -42, 90, 23]
=======
    joints_drink_0 = [-64.92975552277994, -53.757672238791024, 85.59438588088913, -40.594545888330785, 47.02097261708598, 32.10251352756824]
    joints_drink_1 = [-60.67638917551703, 30.958930588437717, 66.30949807520462, -24.253970457265563, 88.6308377313422, 143.5199429315394]
    # [-90,-32, 16, -42, 90, 23]
>>>>>>> 7ab8b32aacdca90fbd07e836fad97c25c5aa0f47

    joints_0 = [-92.7577702283026, -30.702823986508566, 88.30935886238296, -49.02629049397884, 32.89250812404119, 28.075834772565035]
    joints_1 = [-80.41020607088991, -34.8470549319062, 74.18243584578649, -61.832637144726654, 45.022171536507656, 20.512724040675966]
    joints_3 = [-7.974800958608117, -0.4540214624496332, 122.59586252592715, -54.91805498002628, -97.28796424190058, 28.925238904347932]
    joints_2 =  [-9.077455980620101, -8.274095165323153, 127.31495044430211, -42.36492435502244, -98.38927518926192, 28.969842332936764]
    joints_4 = [-30.383106264072968, 0.41192952217100487, 133.08773590164736, -44.954040094452075, -119.67251862965907, 29.843977111342003]

    # 抓杯子预备位置
    Arm.MovebyJoint(joints_drink_0,0.5)
    # 抓住杯子位置
    Arm.MovebyJoint(joints_0,0.5)
    # 夹爪抓取
    GripperControl.newcontrol(2)
    sleep(0.5)
    # 把杯子移动出来避免碰撞
    Arm.MovebyJoint(joints_1,0.5)  
    # 移动杯子到用户区
    Arm.MovebyJoint(joints_3,0.5)
    # 下降
    Arm.MovebyJoint(joints_2,0.5)
    # 松开杯子
    GripperControl.newcontrol(0)
    sleep(0.5)
    # 后撤避免碰撞
    Arm.MovebyJoint(joints_4,0.5)
    # 初始化位置
    Arm.MovebyJoint(joints_drink_1,0.5)